#include <ros/ros.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <octomap/OcTreeBase.h>
#include <fstream>
int main(int argc, char** argv) {
    ros::init(argc, argv, "oct_conver_cloud");
    ros::NodeHandle nh;

    // 创建发布者
    ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("new_cloud", 1);

    // 读取.bt文件路径（请修改为你的实际路径）
    std::string filename = "/home/cxr/wujie_ws/src/pcd_pkg/abc.bt";

    // 加载OctoMap
    octomap::OcTree tree(filename);

    // 创建PCL点云容器
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud;

    // 遍历所有占据的体素
    for (octomap::OcTree::leaf_iterator it = tree.begin_leafs(), end = tree.end_leafs(); it != end; ++it) {
        if (tree.isNodeOccupied(*it)) {
            // 获取体素中心坐标
            octomap::point3d point = it.getCoordinate();
            pcl_cloud.push_back(pcl::PointXYZ(it.getX(), it.getY(), it.getZ()));
        }
    }

    // 转换为ROS消息
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(pcl_cloud, cloud_msg);
    cloud_msg.header.frame_id = "map";  // 请根据实际情况修改坐标系
    cloud_msg.header.stamp = ros::Time::now();

    ROS_INFO("Converted %lu points", pcl_cloud.size());

    ros::Rate rate(1);  // 发布频率1Hz
    while (ros::ok()) {
        cloud_msg.header.stamp = ros::Time::now();
        pub.publish(cloud_msg);
        rate.sleep();
    }

    return 0;
}